#include "library.h"
#include "constants.h"
#include "OdomMove.h"
#include "MyPose.h"
#include "Robot.h"
#include "PoseRange.h"

#ifndef MOTIONMODEL_H_
#define MOTIONMODEL_H_

class MotionModel {
	public:
		MotionModel();
		virtual ~MotionModel();
		void stepRobot(PoseArray &paNow, const OdomMove &om, const Robot &robot, PoseArray &paNext, MATRIX &map);
		void stepRobot2(PoseArray &paNow, const OdomMove &om, const Robot &robot, PoseArray &paNext, MATRIX &map);
		void stepHuman(PoseArray &paNow, PoseArray &paNext, MATRIX &map);

	private:
		Robot config;
		tf::TransformListener listener;
		typedef double GaussianVariance;
		GaussianVariance gp;
		bool validParticle(MyPose &pose,MATRIX &map);
};
#endif /* MOTIONMODEL_H_ */
